namespace il
{
	using namespace std;
	using namespace cv;
	using namespace TCLAP;

	////////////////////////////////////////////////////////
	//////////////Defaults//////////////////////////////////
	////////////////////////////////////////////////////////
	static const int _VERBOSE = 1;
	static const float _MARGIN = 1;
	static const int _K = 3;

	static const int GD = 0;
	static const int ORIENTEDLSS = 1;
	static const int BLINDLSS = 2;
	static const int DIFFERENTIALEVOLUTION = 3;
	static const int CMAES = 4;
	static const int _SOLVER = CMAES;

	static const int _POLY_ORDER = 3;
	static const int  _POPULATION_SIZE = 100;
	static const int _GENERATIONS = 10;
	static const string _TRIPLETS_FILE = "active_elements.xml";
	//////////////////////////////////////////////////////////

	//////////////////////////////////////////////////////////
	//Debug helping functions
	//////////////////////////////////////////////////////////
	
	template<typename T>
	bool print_mat(Mat_<T> mat)
	{
		for(int i = 0 ; i < mat.rows ; ++i)
		{
			cout << "|";
			for(int j = 0 ; j < mat.cols ; ++j)
				cout << mat(i,j) << " ";
			cout <<"|\n";
		}
		return true;
	}

	template<typename T>
	bool check_mat(Mat_<T> mat)//returns false if mat has NANs elements (to be used inside assert)
	{
		for(int i = 0 ; i < mat.rows ; ++i)
		{
			for(int j = 0 ; j < mat.cols ; ++j)
			{
				if( !check_number<T>(mat(i,j)) )
				{
					cout << "Detected NAN elements in matrix.\n";
					print_mat<T>(mat);
					return false;
				}
			}
		}
		return true;
	}

	template<typename T>
	bool check_number(T &x) //return false if x is NAN
	{
		return (x==x);//NANs
	}

	////////////////////////////////////////////////////////////
	//Util
	///////////////////////////////////////////////////////////
	
	float d2(Mat &a, Mat &b)
	{
		Mat_<float> d = a-b;
		return (float)d.dot(d);
	}

	Mat_<float> compute_scaling_params( const Mat_<float> &x)
	{

		Mat_<float> p( 4, x.cols );
		reduce( x, p.row(0), 0, CV_REDUCE_AVG );
		Mat_<float> aux( x.size() );

		for(int i = 0 ; i < x.rows ; ++i)
			aux.row(i) = x.row(i) - p.row(0);

		reduce( aux, p.row(1), 0, CV_REDUCE_MIN);
		reduce( aux, p.row(2), 0, CV_REDUCE_MAX);
		p.row(3) = p.row(2) - p.row(1);

		if(countNonZero(p.row(3))-p.cols)
		{
			cout << "Data contains constant cols that should be ignored.\n";
		}

		return p;
	}

	Mat_<float> rescale( const Mat_<float> &params, const Mat_<float> &x)
	{

		Mat_<float> r( x.size() );
		if( params.cols != r.cols )
			throw exception( "Rescaling problem.", __LINE__ );

		for(int i = 0 ; i < r.rows ; ++i)
			for(int j = 0 ; j < r.cols ; ++j)
				if(params(3,j)>0)
					r(i,j) = 2*(r(i,j)-params(0,j)-params(1,j))/(params(3,j)) - 1;
				else
					r(i,j) = 0;
		return r;

	}

	void shuffle_data_set(const Mat &x1, const Mat &y1, Mat &x2, Mat &y2) 
	{

		Mat_<int> perm(x1.rows,1);
		
		for(int i = 0 ; i < x1.rows ; ++i)
			perm(i,0)=i;

		randShuffle(perm,5);
		
		x1.copyTo(x2);
		y1.copyTo(y2);

		for(int i = 0 ; i < x1.rows ; ++i)
		{
			x1.row(perm(i,0)).copyTo(x2.row(i));
			y1.row(perm(i,0)).copyTo(y2.row(i));
		}

	}

	void split_data_set(Mat &x1, Mat &y1, Mat &x2, Mat &y2, const double &at)
	{

		int first = int(at*x1.rows);
		int second = x1.rows - first;
		assert(first+second==x1.rows);

		shuffle_data_set(x1,y1,x2,y2);

		x1 = x2(Range(0,first),Range(0,x2.cols));
		y1 = y2(Range(0,first),Range(0,y2.cols));

		x2 = x2(Range(first,x2.rows),Range(0,x2.cols));
		y2 = y2(Range(first,y2.rows),Range(0,y2.cols));
		
	}

	class IClassifier
	{
	public: 
		IClassifier(){}
		virtual int operator()(const Mat_<float> &x)=0;
		virtual ~ IClassifier(){}
	};

	class WrapSVM: public SVM, public IClassifier
	{
	public:
		WrapSVM(const Mat &train_data, const Mat &responses, const double frac = 0.2 )
		{
			svmparams.svm_type = SVM::C_SVC;
			svmparams.kernel_type = SVM::LINEAR;
			
			//Do train auto to find optimal parameters using only 100*frac%
			//of the data because it can be really time consuming
			Mat strain,sresp;
			shuffle_data_set(train_data,responses,strain,sresp);

			int s = int(frac*train_data.rows);
			cout << "Optimizing SVM params...\n";
			train_auto(	strain(Range(0,s), Range(0,train_data.cols)),
						sresp(Range(0,s), Range(0,1)),
						Mat(), Mat(), svmparams, 5 );

			//Use the optimal params to train with the whole dataset
			svmparams = this->get_params();
			cout << "Training SVM...\n";
			this->train( strain, sresp, Mat(), Mat(), svmparams);

		}
		virtual ~WrapSVM()
		{
			this->clear();
		}
	
		int operator()(const Mat_<float> &x)
		{
			return int(this->predict(x));
		}

	protected:
		SVMParams svmparams;
	};

	class WrapKNN: public flann::Index, public IClassifier
	{
	public:
		
		WrapKNN(const Mat &train_data, const Mat &responses, const int k=3) : flann::Index( train_data, cv::flann::KDTreeIndexParams() )
		{

			_k = k;
			_y = responses;
			list<int> classlist;

			for(int i = 0 ; i < _y.rows ; ++i)
				classlist.push_back( _y(i,0) );

			classlist.sort();
			classlist.unique();

			//and just because is nice to be able to index in the following loop:
			classes.assign( classlist.begin(), classlist.end() ); 
			classlist.clear();

			indices.create(1,_k);
			dists.create( indices.size() );
			votes.assign(classes.size(),0 );
			
		}

		~WrapKNN()
		{
			classes.clear();
		}

		int operator()(const Mat_<float> &x)
		{
			if(x.rows>1)
				throw exception("WrapKNN's operator() expects a single sample (x.rows==1)", __LINE__);

			knnSearch(x, indices, dists, _k, flann::SearchParams() );

			for(int j = 0 ; j < votes.size(); ++j)
				votes[j] = 0;
			
			for(int j = 0 ; j < _k; ++j)
				for(unsigned int k = 0 ; k < classes.size() ; ++k)
					if(_y(indices(0,j),0) == classes[k])
						votes[k]++;
			
			unsigned int c = 0, k = 0;
			
			while(k<classes.size())
			{
				if(votes[k]>votes[c])
					c = k;
				++k;
			}

			return classes[c];
		}

	protected:
		int _k;
		Mat_<int> _y,indices;
		vector<int> classes,votes;
		Mat_<float> dists;
	};

	int test_wrapknn()
	{
		Mat_<float> a(2,2);
		a = Mat_<float>::eye(2,2);
		flann::Index index(a,flann::KDTreeIndexParams());
	
		Mat_<float> query(1,2),dists(1,1);
		query = 0;
		query(0,0) = 3;
	
		Mat_<int> indices(1,1);
		std::cout << "just checking...";
		index.knnSearch(query,indices,dists,1, cv::flann::SearchParams());
		cout << "indices " << indices(0,0) << " " << dists(0,0) << endl; 
		return 0;
	}

	int test_WrapKNN_consistency(int argc, char **argv)
	{
		Mat_<float> samples(3000,10);
		randn(samples(Range(0,1000),Range(0,10)),Scalar::all(0),Scalar::all(1));
		randn(samples(Range(1000,2000),Range(0,10)),Scalar::all(1),Scalar::all(1));
		randn(samples(Range(2000,3000),Range(0,10)),Scalar::all(-1),Scalar::all(.5));

		Mat_<int> responses(3000,1);
		responses(Range(0,1000), Range(0,1) ) = 1;
		responses(Range(1000,2000), Range(0,1) ) = 2;
		responses(Range(2000,3000), Range(0,1) ) = 3;

		KNearest knearest(samples,responses); //This guy works right but has a memory leak
		WrapKNN wrapknn(samples,responses);//This guy doesn't have memory issues but I don't know if works right

		//Produce test samples
		randn(samples(Range(0,1000),Range(0,10)),Scalar::all(0),Scalar::all(1));
		randn(samples(Range(1000,2000),Range(0,10)),Scalar::all(1),Scalar::all(1));
		randn(samples(Range(2000,3000),Range(0,10)),Scalar::all(-1),Scalar::all(.5));

		unsigned int inconsistencies = 0;
		for(unsigned int i = 0 ; i < samples.rows ; ++i)
		{
			int knearest_resp = (int)knearest.find_nearest(samples.row(i),3);
			int wrapknn_resp = wrapknn(samples.row(i));
			if(knearest_resp != wrapknn_resp)
				++inconsistencies;
		}
		
		cout << "Number of inconsistencies: " << inconsistencies << endl;
		return 0;
	}

	int test_SVM_memory()
	{
		Mat_<float> samples(3000,1000);
		randn(samples(Range(0,1000),Range(0,1000)),Scalar::all(0),Scalar::all(1));
		randn(samples(Range(1000,2000),Range(0,1000)),Scalar::all(1),Scalar::all(1));
		randn(samples(Range(2000,3000),Range(0,1000)),Scalar::all(-1),Scalar::all(.5));

		Mat_<int> responses(3000,1);
		responses(Range(0,1000), Range(0,1) ) = 1;
		responses(Range(1000,2000), Range(0,1) ) = 2;
		responses(Range(2000,3000), Range(0,1) ) = 3;
		SVMParams params;
		params.svm_type = SVM::C_SVC;
		params.kernel_type = SVM::RBF;
		params.C = 2;
		params.gamma = .1;
		
		while(true)
		{
			cout << "Training.."<<endl;
			SVM svm;
			svm.train(samples,responses,Mat(),Mat(),params);
			//WrapSVM svm(samples,responses);
		}

		return 0;
	}

	////////////////////////////////////////////////////////////
	//Functions for reading svmlight format files
	////////////////////////////////////////////////////////////
	struct x_svm_node
	{
		int idx;	// index
		float val;	// value
	};
	
	Mat_<float> svmlight2cvmat( struct x_svm_node** x, const int dim, const int n )
	{
		Mat_<float> samples = Mat_<float>::zeros( n, dim );
		for(int i = 0 ; i < n ; ++i)
		{
			int j = 0;
			while( x[i][j].idx >= 0 )
			{
				samples(i,x[i][j].idx-1) = x[i][j].val;
				++j;
			}
		}
		return samples;
	}

	int read_svmlight( const char* input_file, struct x_svm_node** &_x,
						int* &_y, int &n )
	{
		int features, max_index, i, j, eof;
		FILE * fp = fopen (input_file, "r");
		if (fp == NULL)
		{
			printf ("cannot open file %s", input_file);
			return -1;
		}

		n = 0;
		features = 0;
		eof = 0;
		while (!eof)
		{
			int c = fgetc (fp);
			switch (c)
			{
				// count also the -1 feature 
				case '\n':
					n ++;
				case ':':
					features ++;
					break;
				case EOF:
					eof = 1;
					break;
			}
		}
		rewind (fp);

		// allocate labels, pointers to patterns and all patterns
		_y = new int[n];
		_x = new struct x_svm_node*[n]; 
		struct x_svm_node* x_all = new struct x_svm_node[features];

		max_index = 1;
		j = 0;
		for ( i = 0 ; i < n ; i++ )
		{
			int label;
			_x[i] = &x_all[j];
			fscanf (fp, "%d", &label);
			_y[i] = label;

			while (1)
			{
				int c = fgetc (fp);
				while (isspace (c) && c != '\n')
					c = fgetc (fp);
				if (c == '\n' || c == EOF)
					break;

				// we found something else than whitespace or '\n'
				ungetc (c, fp);
				if ( fscanf (fp, "%d:%f", &(x_all[j].idx), &(x_all[j].val)) < 2 )
				{
					printf ( "invalid format at line %d in file %s", i+1, input_file );
					return -1;
				}
				j ++;
			}

			if ( j >=1 && x_all[j-1].idx > max_index )
				max_index = x_all[j-1].idx;

			x_all[j].idx = -1;
			x_all[j].val = 0;

			j++;
		}

		fclose (fp);

		return max_index;
	}

	int read_from_svmlight_formated_file( const char* input_file,
										   Mat_<float> &samples,
											 Mat_<int> &responses )
	{
		cout << "Loading data from file " << input_file ;

		int n;
		struct x_svm_node **_x;
		int* _y;
		int dimension = read_svmlight( input_file, _x, _y , n );
	
		samples = svmlight2cvmat( _x, dimension , n);
		
		responses.create( n, 1 );
		responses.data = (uchar*)_y;

		cout << " ...DONE || dimension: " << dimension << endl;
	
		return dimension;
	}

}

#include "YAGOSolver.h"
#include "DESolver.h"
#include "CMASolver.h"
#include "datamap.h"
#include "wormholing.h"
#include "lss.h"
#include "costafilter.h"